#include <Arduino.h>
#include <Wire.h>

#define DEBUG_MODE 1          // 调试模式开关
#define PCA9685_ADDR1 0x40    // 第一个PCA9685地址（A0-A4全接地）
#define PCA9685_ADDR2 0x41    // 第二个PCA9685地址（A0接VCC）
#define SERVO_MIN_PULSE 500   // 0度脉冲宽度（微秒）
#define SERVO_MAX_PULSE 2500  // 180度脉冲宽度（微秒）

// 函数声明
void setupPCA9685(uint8_t addr);
void setPWMFreq(uint8_t addr, float freq);
void setServoAngle(uint8_t addr, uint8_t channel, uint8_t angle);

void setup() {
  #if DEBUG_MODE
  Serial.begin(115200);
  delay(1000);  // 等待串口稳定
  #endif

  Wire.begin(21, 22);        // SDA=21, SCL=22 (ESP32 示例)
  Wire.setClock(100000);     // 降低I2C时钟频率至100kHz
  
  // 初始化两个PCA9685
  setupPCA9685(PCA9685_ADDR1);
  setupPCA9685(PCA9685_ADDR2);
  
  #if DEBUG_MODE
  Serial.println("系统初始化完成，32通道已启用");
  #endif
}

void loop() {
  static uint8_t fixedAngle = 90;  // 固定角度为90度

  // 设置所有32个通道的角度（2个PCA9685 × 16通道）
  for (uint8_t ch = 0; ch < 16; ch++) {
    setServoAngle(PCA9685_ADDR1, ch, fixedAngle);  // 第一个PCA9685
    setServoAngle(PCA9685_ADDR2, ch, fixedAngle);  // 第二个PCA9685
  }

  #if DEBUG_MODE
  static unsigned long lastPrint = 0;
  if (millis() - lastPrint > 10000) {  // 每10秒打印一次
    Serial.print("所有舵机设置为: ");
    Serial.print(fixedAngle);
    Serial.println("°");
    lastPrint = millis();
  }
  #endif

  // 不需要频繁重复设置，可适当延时或省略
  delay(1000);  // 可选延迟
}

// 初始化PCA9685（参数化地址）
void setupPCA9685(uint8_t addr) {
  // 多次尝试初始化
  for (int i = 0; i < 3; i++) {
    Wire.beginTransmission(addr);
    Wire.write(0x00);  // MODE1寄存器
    Wire.write(0x20);  // 启用自动递增(AI)
    if (Wire.endTransmission() == 0) break;
    delay(100);
  }
  
  // 设置MODE2寄存器
  Wire.beginTransmission(addr);
  Wire.write(0x01);    // MODE2寄存器
  Wire.write(0x04);    // OUTDRV=1（推挽输出）
  Wire.endTransmission();
  
  setPWMFreq(addr, 50);      // 设置50Hz频率
  delay(50);          // 短延迟

  // 启用所有通道输出
  Wire.beginTransmission(addr);
  Wire.write(0xFA);    // ALL_LED_ON_L寄存器
  Wire.write(0x00);    // ON时间低字节
  Wire.write(0x00);    // ON时间高字节
  Wire.write(0x00);    // OFF时间低字节
  Wire.write(0x00);    // OFF时间高字节
  Wire.endTransmission();

  #if DEBUG_MODE
  Serial.print("PCA9685 @ 0x");
  Serial.print(addr, HEX);
  Serial.println(" 初始化完成");
  #endif
}

// PWM频率设置（参数化地址）
void setPWMFreq(uint8_t addr, float freq) {
  freq = constrain(freq, 24, 1526);
  uint8_t prescale = round(25000000.0 / (4096 * freq)) - 1;

  Wire.beginTransmission(addr);
  Wire.write(0x00);    // 进入睡眠模式
  Wire.write(0x10);    // SLEEP=1
  Wire.endTransmission();

  Wire.beginTransmission(addr);
  Wire.write(0xFE);    // PRE_SCALE寄存器
  Wire.write(prescale);
  Wire.endTransmission();

  Wire.beginTransmission(addr);
  Wire.write(0x00);    // 唤醒
  Wire.write(0x20);    // SLEEP=0, AI=1
  Wire.endTransmission();
}

// 舵机角度控制（支持指定PCA9685地址和通道）
void setServoAngle(uint8_t addr, uint8_t channel, uint8_t angle) {
  angle = constrain(angle, 0, 180);
  uint16_t pulse = map(angle, 0, 180, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
  uint16_t ticks = (uint16_t)(pulse * 4096 / 20000.0);

  // 三次重试机制
  for (int i = 0; i < 3; i++) {
    Wire.beginTransmission(addr);
    Wire.write(0x06 + 4 * channel);  // LEDn_ON_L寄存器
    Wire.write(0x00);                // ON时间低字节
    Wire.write(0x00);                // ON时间高字节
    Wire.write(ticks & 0xFF);        // OFF时间低字节
    Wire.write((ticks >> 8) & 0x0F); // OFF时间高字节
    if (Wire.endTransmission() == 0) break;
    delay(10);
  }

  #if DEBUG_MODE
  if (channel == 0) {  // 仅打印每个PCA9685的通道0信息
    Serial.print("[0x");
    Serial.print(addr, HEX);
    Serial.print(" CH0] Angle:");
    Serial.print(angle);
    Serial.print("° Pulse:");
    Serial.print(pulse);
    Serial.print("us Ticks:");
    Serial.println(ticks);
  }
  #endif
}